{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# ReferenceFrameFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "`ReferenceFrameFactor<POINT, TRANSFORM>` is a ternary factor used to relate landmark observations made in two different reference frames (e.g., from two different robots or two distinct SLAM sessions).\n",
        "It connects:\n",
        "1.  A landmark (`POINT`) expressed in a 'global' or common reference frame (`globalKey`).\n",
        "2.  A transform (`TRANSFORM`) representing the pose of a 'local' frame relative to the 'global' frame (`transKey`). Typically `TRANSFORM = global_T_local`.\n",
        "3.  The *same* landmark (`POINT`) expressed in the 'local' reference frame (`localKey`).\n",
        "\n",
        "The factor enforces the constraint that the globally expressed landmark, when transformed by the `global_T_local` transform, should match the locally expressed landmark.\n",
        "The transformation logic depends on the specific `POINT` and `TRANSFORM` types and is handled by the `transform_point` helper function (which usually calls `Transform::transformFrom`).\n",
        "\n",
        "Error: $ \\text{Log}(\\text{local}^{-1} \\cdot \\text{transform\\_point}(\\text{trans}, \\text{global})) $\n",
        "\n",
        "This factor is crucial for multi-robot map merging or combining results from different SLAM sessions where common landmarks have been observed."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/ReferenceFrameFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "['ReferenceFrameFactorPoint3Pose3']\n"
          ]
        }
      ],
      "source": [
        "import gtsam\n",
        "print([k for k in gtsam.__dict__.keys() if \"ReferenceFrame\" in k])"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import Pose3, Point3, Rot3, Values, NonlinearFactorGraph\n",
        "# The Python wrapper creates specific instantiations\n",
        "from gtsam import ReferenceFrameFactorPoint3Pose3\n",
        "from gtsam.symbol_shorthand import L, T, O"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_header_md"
      },
      "source": [
        "## Creating a ReferenceFrameFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_desc_md"
      },
      "source": [
        "Instantiate by providing the keys for the global landmark, the transform, the local landmark, and a noise model.\n",
        "The noise model dimensionality should match the dimension of the `POINT` type."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "id": "create_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "ReferenceFrameFactor: : ReferenceFrameFactor(Global: l0, Transform: t0, Local: o0)\n",
            "isotropic dim=3 sigma=0.1\n"
          ]
        }
      ],
      "source": [
        "global_landmark_key = L(0)\n",
        "transform_key = T(0)\n",
        "local_landmark_key = O(0)\n",
        "\n",
        "# Noise model on the landmark point difference (e.g., Point3 -> 3 dims)\n",
        "noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) # 10cm std dev\n",
        "\n",
        "# Factor type includes Point and Transform types\n",
        "factor = ReferenceFrameFactorPoint3Pose3(global_landmark_key,\n",
        "                                         transform_key,\n",
        "                                         local_landmark_key,\n",
        "                                         noise)\n",
        "factor.print(\"ReferenceFrameFactor: \")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_header_md"
      },
      "source": [
        "## Evaluating the Error"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_desc_md"
      },
      "source": [
        "The error represents how much the transformed global landmark deviates from the local landmark estimate."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "id": "eval_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Expected local landmark: [ 2. -4.  1.]\n",
            "\n",
            "Error at ground truth: 4500.0 (Should be zero)\n",
            "Error with noisy local landmark: 4621.125\n"
          ]
        }
      ],
      "source": [
        "values = Values()\n",
        "\n",
        "# Example values\n",
        "global_landmark = Point3(5.0, 2.0, 1.0)\n",
        "global_T_local = Pose3(Rot3.Yaw(np.pi/2), Point3(1, 0, 0))\n",
        "expected_local_landmark = global_T_local.transformTo(global_landmark)\n",
        "print(f\"Expected local landmark: {expected_local_landmark}\")\n",
        "\n",
        "values.insert(global_landmark_key, global_landmark)\n",
        "values.insert(transform_key, global_T_local)\n",
        "values.insert(local_landmark_key, expected_local_landmark)\n",
        "\n",
        "error_gt = factor.error(values)\n",
        "print(f\"\\nError at ground truth: {error_gt} (Should be zero)\")\n",
        "\n",
        "# Introduce error in the local landmark estimate\n",
        "noisy_local_landmark = expected_local_landmark + Point3(0.1, -0.1, 0.05)\n",
        "values.update(local_landmark_key, noisy_local_landmark)\n",
        "error_noisy = factor.error(values)\n",
        "print(f\"Error with noisy local landmark: {error_noisy}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "gtsam",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.13.1"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
